/**
 * @file      : project_core.c
 * @brief     : 电子控制板核心代码 - 论文附录
 * @copyright : 米醋电子工作室
 */

/* 包含头文件 */
#include "stm32f4xx_hal.h"
#include "ssd1306.h"
#include "multitimer.h"
#include "motor.h"
#include "emm_v5.h"
#include "rt_ringbuffer.h"
#include "pid.h"
#include "flash.h"
#include "camera.h"
#include "usart.h"
#include "dma.h"
#include "gpio.h"
#include "spi.h"
#include "delay.h"
#include "position.h"
#include <stdio.h>
#include <string.h>
#include <math.h>

/* 全局定义 */
#define OLED_TASK_TIME 500  // OLED刷新时间间隔(ms)
#define MOTOR_TASK_TIME 20  // 电机控制时间间隔(ms)
#define PID_TASK_TIME 10    // PID计算时间间隔(ms)
#define CAMERA_TASK_TIME 50 // 摄像头数据处理间隔(ms)
#define USER_TASK_TIME 100  // 用户交互时间间隔(ms)
#define UART_TIMEOUT 100    // 串口超时时间(ms)

#define X_MOTOR_ADDR 0x01        // X轴电机地址
#define Y_MOTOR_ADDR 0x01        // Y轴电机地址
#define MAX_POSITION_X 10000     // X轴最大位置
#define MAX_POSITION_Y 10000     // Y轴最大位置
#define MOTOR_SPEED_DEFAULT 1000 // 默认电机速度
#define MOTOR_ACC_DEFAULT 500    // 默认电机加速度

#define PID_KP 1.5f         // PID比例系数
#define PID_KI 0.05f        // PID积分系数
#define PID_KD 0.2f         // PID微分系数
#define PID_MAX_OUTPUT 5000 // PID最大输出

#define FLASH_SECTOR FLASH_SECTOR_11 // Flash存储扇区
#define FLASH_ADDRESS 0x080E0000     // Flash存储地址

#define CAM_THRESHOLD 128 // 摄像头图像二值化阈值
#define CAM_WIDTH 320     // 摄像头图像宽度
#define CAM_HEIGHT 240    // 摄像头图像高度

/* 全局变量定义 */
// 多任务定时器
MultiTimer mt_system, mt_oled, mt_usart, mt_cam, mt_pid, mt_user;

// 环形缓冲区
struct rt_ringbuffer ringbuffer_x, ringbuffer_y, ringbuffer_cam, ringbuffer_user;
uint8_t ringbuffer_pool_x[256], ringbuffer_pool_y[256];
uint8_t ringbuffer_pool_cam[512], ringbuffer_pool_user[256];

// 系统状态变量
__IO uint32_t count = 0;
char buffer[20];

// 电机相关变量
typedef struct
{
    int32_t current_pos; // 当前位置
    int32_t target_pos;  // 目标位置
    int32_t initial_pos; // 初始位置
    uint16_t speed;      // 速度
    uint16_t acc;        // 加速度
    uint8_t is_running;  // 运行状态
} MotorParam_t;

MotorParam_t motor_x, motor_y;

// PID控制器
typedef struct
{
    float kp;         // 比例系数
    float ki;         // 积分系数
    float kd;         // 微分系数
    float error;      // 当前误差
    float last_error; // 上次误差
    float integral;   // 积分值
    float output;     // 输出值
    float max_output; // 最大输出限制
} PID_t;

PID_t pid_x, pid_y;

// 摄像头数据处理变量
typedef struct
{
    uint8_t frame_buffer[CAM_WIDTH * CAM_HEIGHT / 8]; // 二值化后的图像缓冲
    int16_t center_x;                                 // 检测到的目标中心X坐标
    int16_t center_y;                                 // 检测到的目标中心Y坐标
    uint8_t target_found;                             // 是否检测到目标
} Camera_t;

Camera_t camera_data;

// 系统配置
typedef struct
{
    uint32_t magic_number; // 魔术数字，用于验证配置有效性
    MotorParam_t saved_x;  // 保存的X电机参数
    MotorParam_t saved_y;  // 保存的Y电机参数
    uint8_t mode;          // 系统工作模式
    uint8_t auto_track;    // 自动跟踪使能标志
    uint16_t checksum;     // 校验和
} SystemConfig_t;

SystemConfig_t sys_config = {
    .magic_number = 0xA5A5A5A5,
    .mode = 0,
    .auto_track = 0};

/**
 * @brief 获取系统时钟,为多定时器提供基准
 */
uint64_t bsp_get_systick(void)
{
    return (uint64_t)uwTick;
}

/**
 * @brief 计算PID控制输出
 * @param pid PID控制器结构体
 * @param current 当前值
 * @param target 目标值
 * @return 计算后的PID输出值
 */
float calculate_pid(PID_t *pid, float current, float target)
{
    pid->error = target - current;
    pid->integral += pid->error;

    // 积分限幅
    if (pid->integral > pid->max_output)
        pid->integral = pid->max_output;
    else if (pid->integral < -pid->max_output)
        pid->integral = -pid->max_output;

    // 计算PID输出
    pid->output = pid->kp * pid->error +
                  pid->ki * pid->integral +
                  pid->kd * (pid->error - pid->last_error);

    // 输出限幅
    if (pid->output > pid->max_output)
        pid->output = pid->max_output;
    else if (pid->output < -pid->max_output)
        pid->output = -pid->max_output;

    pid->last_error = pid->error;

    return pid->output;
}

/**
 * @brief 初始化PID控制器
 * @param pid PID控制器指针
 */
void init_pid(PID_t *pid)
{
    pid->kp = PID_KP;
    pid->ki = PID_KI;
    pid->kd = PID_KD;
    pid->error = 0;
    pid->last_error = 0;
    pid->integral = 0;
    pid->output = 0;
    pid->max_output = PID_MAX_OUTPUT;
}

/**
 * @brief 控制电机运动到指定位置
 * @param huart 串口句柄
 * @param addr 电机地址
 * @param pos 目标位置
 * @param speed 运动速度
 * @param acc 加速度
 */
void motor_move_to_position(UART_HandleTypeDef *huart, uint8_t addr, int32_t pos, uint16_t speed, uint16_t acc)
{
    // 发送电机运动指令
    uint8_t cmd_buffer[20];
    uint8_t len = 0;

    // 命令头
    cmd_buffer[len++] = 0xAA; // 起始字节
    cmd_buffer[len++] = addr; // 地址
    cmd_buffer[len++] = 0x10; // 功能码:绝对定位

    // 位置参数(4字节)
    cmd_buffer[len++] = (pos >> 0) & 0xFF;
    cmd_buffer[len++] = (pos >> 8) & 0xFF;
    cmd_buffer[len++] = (pos >> 16) & 0xFF;
    cmd_buffer[len++] = (pos >> 24) & 0xFF;

    // 速度参数(2字节)
    cmd_buffer[len++] = (speed >> 0) & 0xFF;
    cmd_buffer[len++] = (speed >> 8) & 0xFF;

    // 加速度参数(2字节)
    cmd_buffer[len++] = (acc >> 0) & 0xFF;
    cmd_buffer[len++] = (acc >> 8) & 0xFF;

    // 计算校验和
    uint8_t checksum = 0;
    for (int i = 0; i < len; i++)
    {
        checksum += cmd_buffer[i];
    }
    cmd_buffer[len++] = checksum;

    // 发送命令
    HAL_UART_Transmit(huart, cmd_buffer, len, UART_TIMEOUT);
}

/**
 * @brief 读取电机位置
 * @param huart 串口句柄
 * @param addr 电机地址
 * @return 电机当前位置
 */
int32_t read_motor_position(UART_HandleTypeDef *huart, uint8_t addr)
{
    uint8_t cmd_buffer[5];
    uint8_t recv_buffer[10];
    int32_t position = 0;

    // 构建读取位置命令
    cmd_buffer[0] = 0xAA;                                          // 起始字节
    cmd_buffer[1] = addr;                                          // 地址
    cmd_buffer[2] = 0x20;                                          // 功能码:读取位置
    cmd_buffer[3] = cmd_buffer[0] + cmd_buffer[1] + cmd_buffer[2]; // 校验和

    // 发送命令
    HAL_UART_Transmit(huart, cmd_buffer, 4, UART_TIMEOUT);

    // 接收响应
    if (HAL_UART_Receive(huart, recv_buffer, 9, UART_TIMEOUT) == HAL_OK)
    {
        // 解析位置信息(小端字节序)
        position = (recv_buffer[5] << 24) |
                   (recv_buffer[4] << 16) |
                   (recv_buffer[3] << 8) |
                   (recv_buffer[2]);
    }

    return position;
}

/**
 * @brief 计算校验和
 * @param data 数据缓冲区
 * @param len 数据长度
 * @return 校验和
 */
uint16_t calculate_checksum(uint8_t *data, uint16_t len)
{
    uint16_t sum = 0;
    for (uint16_t i = 0; i < len; i++)
    {
        sum += data[i];
    }
    return sum;
}

/**
 * @brief 保存系统配置到Flash
 */
void save_config_to_flash(void)
{
    // 更新校验和
    sys_config.checksum = calculate_checksum((uint8_t *)&sys_config, sizeof(SystemConfig_t) - 2);

    // 解锁Flash
    HAL_FLASH_Unlock();

    // 擦除扇区
    FLASH_EraseInitTypeDef erase;
    erase.TypeErase = FLASH_TYPEERASE_SECTORS;
    erase.Sector = FLASH_SECTOR;
    erase.NbSectors = 1;
    erase.VoltageRange = FLASH_VOLTAGE_RANGE_3;

    uint32_t sector_error = 0;
    HAL_FLASHEx_Erase(&erase, &sector_error);

    // 写入配置数据
    uint32_t *data_ptr = (uint32_t *)&sys_config;
    uint32_t address = FLASH_ADDRESS;

    for (uint32_t i = 0; i < sizeof(SystemConfig_t) / 4; i++)
    {
        HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, address, data_ptr[i]);
        address += 4;
    }

    // 锁定Flash
    HAL_FLASH_Lock();
}

/**
 * @brief 从Flash加载系统配置
 * @return 1:成功加载 0:加载失败(数据无效)
 */
uint8_t load_config_from_flash(void)
{
    // 从Flash读取配置数据
    memcpy(&sys_config, (void *)FLASH_ADDRESS, sizeof(SystemConfig_t));

    // 验证魔术数字和校验和
    uint16_t calculated_checksum = calculate_checksum((uint8_t *)&sys_config, sizeof(SystemConfig_t) - 2);

    if (sys_config.magic_number == 0xA5A5A5A5 &&
        calculated_checksum == sys_config.checksum)
    {
        // 数据有效,应用配置
        motor_x = sys_config.saved_x;
        motor_y = sys_config.saved_y;
        return 1;
    }

    // 数据无效,使用默认配置
    return 0;
}

/**
 * @brief 保存初始位置
 */
void save_initial_position(void)
{
    // 获取并保存初始位置
    motor_x.initial_pos = read_motor_position(&huart2, X_MOTOR_ADDR);
    motor_y.initial_pos = read_motor_position(&huart5, Y_MOTOR_ADDR);

    // 更新配置
    sys_config.saved_x.initial_pos = motor_x.initial_pos;
    sys_config.saved_y.initial_pos = motor_y.initial_pos;

    // 存储到Flash
    save_config_to_flash();
}

/**
 * @brief 进行图像处理寻找目标
 * @param image 图像数据
 * @param width 图像宽度
 * @param height 图像高度
 */
void process_image(uint8_t *image, uint16_t width, uint16_t height)
{
    int32_t sum_x = 0, sum_y = 0;
    int32_t pixel_count = 0;

    // 计算目标质心
    for (uint16_t y = 0; y < height; y++)
    {
        for (uint16_t x = 0; x < width; x++)
        {
            uint16_t pos = y * width + x;
            uint8_t byte_pos = pos / 8;
            uint8_t bit_pos = pos % 8;

            // 检查像素是否为1(目标点)
            if (image[byte_pos] & (1 << bit_pos))
            {
                sum_x += x;
                sum_y += y;
                pixel_count++;
            }
        }
    }

    // 计算目标中心
    if (pixel_count > 10)
    { // 至少10个像素才认为找到目标
        camera_data.center_x = sum_x / pixel_count;
        camera_data.center_y = sum_y / pixel_count;
        camera_data.target_found = 1;
    }
    else
    {
        camera_data.target_found = 0;
    }
}

/**
 * @brief 处理接收到的摄像头数据
 * @param data 接收到的数据
 */
void process_camera_data(uint8_t *data)
{
    static uint16_t recv_count = 0;
    static uint8_t frame_buffer[CAM_WIDTH * CAM_HEIGHT / 8];

    // 解析摄像头数据帧
    // 假设数据结构: [帧头2字节][数据长度2字节][图像数据][校验和1字节]
    if (data[0] == 0xFF && data[1] == 0xA5)
    {
        uint16_t length = (data[3] << 8) | data[2];

        if (length <= sizeof(frame_buffer))
        {
            // 复制图像数据
            memcpy(frame_buffer, &data[4], length);
            recv_count = length;

            // 处理图像
            if (recv_count == CAM_WIDTH * CAM_HEIGHT / 8)
            {
                process_image(frame_buffer, CAM_WIDTH, CAM_HEIGHT);
                recv_count = 0;
            }
        }
    }
}

/**
 * @brief 自动跟踪控制
 */
void auto_track_control(void)
{
    if (sys_config.auto_track && camera_data.target_found)
    {
        // 计算屏幕中心偏移
        int16_t error_x = camera_data.center_x - CAM_WIDTH / 2;
        int16_t error_y = camera_data.center_y - CAM_HEIGHT / 2;

        // 使用PID计算电机调整量
        float adjust_x = calculate_pid(&pid_x, error_x, 0);
        float adjust_y = calculate_pid(&pid_y, error_y, 0);

        // 更新电机目标位置
        motor_x.target_pos = motor_x.current_pos - (int32_t)adjust_x;
        motor_y.target_pos = motor_y.current_pos - (int32_t)adjust_y;

        // 限制位置范围
        if (motor_x.target_pos > MAX_POSITION_X)
            motor_x.target_pos = MAX_POSITION_X;
        else if (motor_x.target_pos < -MAX_POSITION_X)
            motor_x.target_pos = -MAX_POSITION_X;

        if (motor_y.target_pos > MAX_POSITION_Y)
            motor_y.target_pos = MAX_POSITION_Y;
        else if (motor_y.target_pos < -MAX_POSITION_Y)
            motor_y.target_pos = -MAX_POSITION_Y;

        // 发送电机控制命令
        motor_move_to_position(&huart2, X_MOTOR_ADDR, motor_x.target_pos, motor_x.speed, motor_x.acc);
        motor_move_to_position(&huart5, Y_MOTOR_ADDR, motor_y.target_pos, motor_y.speed, motor_y.acc);
    }
}

/**
 * @brief 处理串口接收数据
 * @param data 接收到的数据
 * @param len 数据长度
 */
void process_uart_data(uint8_t *data, uint16_t len)
{
    if (len > 0 && data[0] == '$')
    {
        switch (data[1])
        {
        case 'M': // 手动控制命令
            if (len >= 6)
            {
                int16_t x_offset = (data[2] << 8) | data[3];
                int16_t y_offset = (data[4] << 8) | data[5];

                // 更新目标位置
                motor_x.target_pos = motor_x.current_pos + x_offset;
                motor_y.target_pos = motor_y.current_pos + y_offset;

                // 发送控制命令
                motor_move_to_position(&huart2, X_MOTOR_ADDR, motor_x.target_pos, motor_x.speed, motor_x.acc);
                motor_move_to_position(&huart5, Y_MOTOR_ADDR, motor_y.target_pos, motor_y.speed, motor_y.acc);
            }
            break;

        case 'H': // 回原点命令
            motor_x.target_pos = motor_x.initial_pos;
            motor_y.target_pos = motor_y.initial_pos;
            motor_move_to_position(&huart2, X_MOTOR_ADDR, motor_x.target_pos, motor_x.speed, motor_x.acc);
            motor_move_to_position(&huart5, Y_MOTOR_ADDR, motor_y.target_pos, motor_y.speed, motor_y.acc);
            break;

        case 'A': // 自动跟踪开关
            if (len >= 3)
            {
                sys_config.auto_track = data[2];
                save_config_to_flash();
            }
            break;

        case 'S': // 保存当前位置为初始位置
            save_initial_position();
            break;

        default:
            break;
        }
    }
}

/**
 * @brief OLED显示任务,定时刷新显示内容
 */
void oled_task(MultiTimer *timer, void *userData)
{
    // 清屏
    ssd1306_basic_clear();

    // 显示计数和位置信息
    sprintf(buffer, "Count: %d", count++);
    ssd1306_basic_string(0, 0, buffer, (uint16_t)strlen(buffer), 1, SSD1306_FONT_12);

    sprintf(buffer, "X: %ld", motor_x.current_pos);
    ssd1306_basic_string(0, 15, buffer, (uint16_t)strlen(buffer), 1, SSD1306_FONT_12);

    sprintf(buffer, "Y: %ld", motor_y.current_pos);
    ssd1306_basic_string(0, 30, buffer, (uint16_t)strlen(buffer), 1, SSD1306_FONT_12);

    // 显示跟踪状态
    sprintf(buffer, "Track: %s", sys_config.auto_track ? "ON" : "OFF");
    ssd1306_basic_string(0, 45, buffer, (uint16_t)strlen(buffer), 1, SSD1306_FONT_12);

    // 重启定时器
    multiTimerStart(timer, OLED_TASK_TIME, oled_task, NULL);
}

/**
 * @brief 电机控制任务,处理伺服电机定位与驱动
 */
void motor_task(MultiTimer *timer, void *userData)
{
    // 读取电机位置
    motor_x.current_pos = read_motor_position(&huart2, X_MOTOR_ADDR);
    motor_y.current_pos = read_motor_position(&huart5, Y_MOTOR_ADDR);

    // 自动跟踪控制
    if (sys_config.auto_track)
    {
        auto_track_control();
    }

    // 重启定时器
    multiTimerStart(timer, MOTOR_TASK_TIME, motor_task, NULL);
}

/**
 * @brief PID控制任务,用于电机精确定位
 */
void pid_task(MultiTimer *timer, void *userData)
{
    uint8_t data_x[8], data_y[8];

    // 从环形缓冲区读取数据
    if (rt_ringbuffer_get(&ringbuffer_x, data_x, sizeof(data_x)) > 0)
    {
        // 处理X轴PID控制
        int16_t target_x = (data_x[0] << 8) | data_x[1];
        float output_x = calculate_pid(&pid_x, motor_x.current_pos, target_x);

        // 应用控制输出
        if (fabs(output_x) > 5.0f)
        { // 避免小幅抖动
            motor_x.target_pos = motor_x.current_pos + (int32_t)output_x;
            motor_move_to_position(&huart2, X_MOTOR_ADDR, motor_x.target_pos, motor_x.speed, motor_x.acc);
        }
    }

    if (rt_ringbuffer_get(&ringbuffer_y, data_y, sizeof(data_y)) > 0)
    {
        // 处理Y轴PID控制
        int16_t target_y = (data_y[0] << 8) | data_y[1];
        float output_y = calculate_pid(&pid_y, motor_y.current_pos, target_y);

        // 应用控制输出
        if (fabs(output_y) > 5.0f)
        { // 避免小幅抖动
            motor_y.target_pos = motor_y.current_pos + (int32_t)output_y;
            motor_move_to_position(&huart5, Y_MOTOR_ADDR, motor_y.target_pos, motor_y.speed, motor_y.acc);
        }
    }

    // 重启定时器
    multiTimerStart(timer, PID_TASK_TIME, pid_task, NULL);
}

/**
 * @brief 摄像头数据处理任务
 */
void camera_task(MultiTimer *timer, void *userData)
{
    uint8_t cam_data[32];

    // 从摄像头缓冲区读取数据
    if (rt_ringbuffer_get(&ringbuffer_cam, cam_data, sizeof(cam_data)) > 0)
    {
        process_camera_data(cam_data);
    }

    // 重启定时器
    multiTimerStart(timer, CAMERA_TASK_TIME, camera_task, NULL);
}

/**
 * @brief 用户交互任务
 */
void user_task(MultiTimer *timer, void *userData)
{
    uint8_t user_data[16];

    // 从用户命令缓冲区读取数据
    if (rt_ringbuffer_get(&ringbuffer_user, user_data, sizeof(user_data)) > 0)
    {
        process_uart_data(user_data, sizeof(user_data));
    }

    // 重启定时器
    multiTimerStart(timer, USER_TASK_TIME, user_task, NULL);
}

/**
 * @brief 系统初始化
 */
void system_init(void)
{
    // 延时初始化
    delay_init();

    // 环形缓冲区初始化
    rt_ringbuffer_init(&ringbuffer_y, ringbuffer_pool_y, sizeof(ringbuffer_pool_y));
    rt_ringbuffer_init(&ringbuffer_x, ringbuffer_pool_x, sizeof(ringbuffer_pool_x));
    rt_ringbuffer_init(&ringbuffer_cam, ringbuffer_pool_cam, sizeof(ringbuffer_pool_cam));
    rt_ringbuffer_init(&ringbuffer_user, ringbuffer_pool_user, sizeof(ringbuffer_pool_user));

    // OLED显示初始化
    ssd1306_basic_init(SSD1306_INTERFACE_SPI, SSD1306_ADDR_SA0_0);
    ssd1306_basic_clear();

    // 电机初始化
    Motor_Init();
    Emm_V5_Reset_CurPos_To_Zero(&huart5, Y_MOTOR_ADDR);
    Emm_V5_Reset_CurPos_To_Zero(&huart2, X_MOTOR_ADDR);

    // 初始化电机参数
    motor_x.speed = MOTOR_SPEED_DEFAULT;
    motor_x.acc = MOTOR_ACC_DEFAULT;
    motor_x.is_running = 0;

    motor_y.speed = MOTOR_SPEED_DEFAULT;
    motor_y.acc = MOTOR_ACC_DEFAULT;
    motor_y.is_running = 0;

    // 初始化PID控制器
    init_pid(&pid_x);
    init_pid(&pid_y);

    // 加载配置
    if (!load_config_from_flash())
    {
        // 配置无效,保存初始位置
        save_initial_position();
    }
}

/**
 * @brief 启动所有任务
 */
void start_all_tasks(void)
{
    // 多任务定时器初始化
    multiTimerInstall(bsp_get_systick);

    // 启动各个任务
    multiTimerStart(&mt_oled, OLED_TASK_TIME, oled_task, NULL);
    multiTimerStart(&mt_pid, PID_TASK_TIME, pid_task, NULL);
    multiTimerStart(&mt_cam, CAMERA_TASK_TIME, camera_task, NULL);
    multiTimerStart(&mt_user, USER_TASK_TIME, user_task, NULL);
    multiTimerStart(&mt_usart, MOTOR_TASK_TIME, motor_task, NULL);
}

/**
 * @brief UART接收回调函数
 */
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    static uint8_t rx_buffer[1];

    if (huart->Instance == USART1)
    {
        // 用户命令接口
        rt_ringbuffer_put(&ringbuffer_user, rx_buffer, 1);
        HAL_UART_Receive_IT(huart, rx_buffer, 1);
    }
    else if (huart->Instance == USART3)
    {
        // 摄像头数据接口
        rt_ringbuffer_put(&ringbuffer_cam, rx_buffer, 1);
        HAL_UART_Receive_IT(huart, rx_buffer, 1);
    }
}

/**
 * @brief 主程序入口
 */
int main(void)
{
    // 外设初始化 (由CubeMX生成)
    HAL_Init();
    SystemClock_Config();
    MX_GPIO_Init();
    MX_DMA_Init();
    MX_UART5_Init();
    MX_USART2_UART_Init();
    MX_SPI1_Init();
    MX_USART1_UART_Init();
    MX_USART3_UART_Init();

    // 系统初始化
    system_init();

    // 启动所有任务
    start_all_tasks();

    // 启动UART中断接收
    HAL_UART_Receive_IT(&huart1, rx_buffer_user, 1);
    HAL_UART_Receive_IT(&huart3, rx_buffer_cam, 1);

    // 主循环
    while (1)
    {
        // 多任务调度
        multiTimerYield();
    }
}